#include "State_Jump2.h"
#include "Dog.h"

#define POINTANGLE RAD(100)//100
#define KP1 19.5f//17.5双木桥  24高栏
#define KD1 0.6f
#define KP2 17.5f//18   24
#define KD2 0.6f
#define jumpAngle RAD(35)

enum jump_state
{
    READY_STATE,
    JUMPING_STATE,
    SWING_STATE,
    FALLING_STATE,
} jumpState;

void Jump2_Enter(void)
{
    UART_Print("Jump2 enter\n");

    /* 蹬腿跳跃 */
    HT_SetPosition(dog.legs[0].motorF, ANGLE_F + global.jumpAngleR + POINTANGLE, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorF, -ANGLE_F - global.jumpAngleL - POINTANGLE, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorF, -ANGLE_F - global.jumpAngleL - POINTANGLE, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorF, ANGLE_F + global.jumpAngleR + POINTANGLE, KP2, KD2, 0);

    HT_SetPosition(dog.legs[0].motorB, -ANGLE_B - global.jumpAngleR + POINTANGLE, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorB, ANGLE_B + global.jumpAngleL - POINTANGLE, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorB, ANGLE_B + global.jumpAngleL - POINTANGLE, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorB, -ANGLE_B - global.jumpAngleR + POINTANGLE, KP2, KD2, 0);

    jumpState = JUMPING_STATE; // 跳跃状态中
}

#define KP3 5.0f//4.0f
#define KD3 0.3f
#define KP4 1.2f
#define KD4 12.0f

void Jump2_Run(void)
{
    // UART_Print("Jump2 run\n");

    switch (jumpState)
    {
    case JUMPING_STATE:
        if (HT_ArrivalPos(dog.legs[0].motorF, 0.1) &&
            HT_ArrivalPos(dog.legs[1].motorF, 0.1) &&
            HT_ArrivalPos(dog.legs[2].motorF, 0.1) &&
            HT_ArrivalPos(dog.legs[3].motorF, 0.1))
        {
            /* 空中摆腿 */
           /* HT_SetPosition(dog.legs[0].motorF, ANGLE_F - global.jumpAngleR, KP2, KD2, 0);
            HT_SetPosition(dog.legs[1].motorF, -ANGLE_F + global.jumpAngleL, KP2, KD2, 0);
            HT_SetPosition(dog.legs[2].motorF, -ANGLE_F + global.jumpAngleL, KP2, KD2, 0);
            HT_SetPosition(dog.legs[3].motorF, ANGLE_F - global.jumpAngleR, KP2, KD2, 0);

            HT_SetPosition(dog.legs[0].motorB, -ANGLE_B + global.jumpAngleR, KP2, KD2, 0);
            HT_SetPosition(dog.legs[1].motorB, ANGLE_B - global.jumpAngleL, KP2, KD2, 0);
            HT_SetPosition(dog.legs[2].motorB, ANGLE_B - global.jumpAngleL, KP2, KD2, 0);
            HT_SetPosition(dog.legs[3].motorB, -ANGLE_B + global.jumpAngleR, KP2, KD2, 0);*/

             HT_SetPosition(dog.legs[0].motorF, ANGLE_F - jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[1].motorF, -ANGLE_F + jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[2].motorF, -ANGLE_F + jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[3].motorF, ANGLE_F - jumpAngle, KP3, KD3, 0);

            HT_SetPosition(dog.legs[0].motorB, -ANGLE_B + jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[1].motorB, ANGLE_B - jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[2].motorB, ANGLE_B - jumpAngle, KP3, KD3, 0);
            HT_SetPosition(dog.legs[3].motorB, -ANGLE_B + jumpAngle, KP3, KD3, 0);

            
             /*HT_SetPosition(dog.legs[0].motorF, ANGLE_F, KP2, KD2, 0);
            HT_SetPosition(dog.legs[1].motorF, -ANGLE_F, KP2, KD2, 0);
            HT_SetPosition(dog.legs[2].motorF, -ANGLE_F, KP2, KD2, 0);
            HT_SetPosition(dog.legs[3].motorF, ANGLE_F, KP2, KD2, 0);

            HT_SetPosition(dog.legs[0].motorB, -ANGLE_B, KP2, KD2, 0);
            HT_SetPosition(dog.legs[1].motorB, ANGLE_B , KP2, KD2, 0);
            HT_SetPosition(dog.legs[2].motorB, ANGLE_B , KP2, KD2, 0);
            HT_SetPosition(dog.legs[3].motorB, -ANGLE_B, KP2, KD2, 0);*/

            jumpState = SWING_STATE; // 摆动状态中
        }
        break;

    case SWING_STATE:
        if (HT_ArrivalPos(dog.legs[0].motorF, 0.2) &&
            HT_ArrivalPos(dog.legs[1].motorF, 0.2) &&
            HT_ArrivalPos(dog.legs[2].motorF, 0.2) &&
            HT_ArrivalPos(dog.legs[3].motorF, 0.2))
        {
            /* 落地阻尼 */
            /*HT_SetPosition(dog.legs[0].motorF, ANGLE_F - global.jumpAngleR, KP3, KD3, 0);
            HT_SetPosition(dog.legs[1].motorF, -ANGLE_F + global.jumpAngleL, KP3, KD3, 0);
            HT_SetPosition(dog.legs[2].motorF, -ANGLE_F + global.jumpAngleL, KP3, KD3, 0);
            HT_SetPosition(dog.legs[3].motorF, ANGLE_F - global.jumpAngleR, KP3, KD3, 0);

            HT_SetPosition(dog.legs[0].motorB, -ANGLE_B + global.jumpAngleR, KP3, KD3, 0);
            HT_SetPosition(dog.legs[1].motorB, ANGLE_B - global.jumpAngleL, KP3, KD3, 0);
            HT_SetPosition(dog.legs[2].motorB, ANGLE_B - global.jumpAngleL, KP3, KD3, 0);
            HT_SetPosition(dog.legs[3].motorB, -ANGLE_B + global.jumpAngleR, KP3, KD3, 0);*/

            HT_SetPosition(dog.legs[0].motorF, ANGLE_F - jumpAngle, KP4, KD4, 0);
            HT_SetPosition(dog.legs[1].motorF, -ANGLE_F + jumpAngle, KP4, KD4, 0);
            HT_SetPosition(dog.legs[2].motorF, -ANGLE_F + jumpAngle, KP4, KD4, 0);
            HT_SetPosition(dog.legs[3].motorF, ANGLE_F - jumpAngle, KP4, KD4, 0);

            HT_SetPosition(dog.legs[0].motorB, -ANGLE_B + jumpAngle, KP4, KD4, 0);
            HT_SetPosition(dog.legs[1].motorB, ANGLE_B - jumpAngle, KP4, KD4, 0);
            HT_SetPosition(dog.legs[2].motorB, ANGLE_B - jumpAngle, KP4, KD4, 0);
            HT_SetPosition(dog.legs[3].motorB, -ANGLE_B + jumpAngle, KP4, KD4, 0);

           /* HT_SetPosition(dog.legs[0].motorF, ANGLE_F, KP3, KD3, 0);
            HT_SetPosition(dog.legs[1].motorF, -ANGLE_F, KP3, KD3, 0);
            HT_SetPosition(dog.legs[2].motorF, -ANGLE_F, KP3, KD3, 0);
            HT_SetPosition(dog.legs[3].motorF, ANGLE_F, KP3, KD3, 0);

            HT_SetPosition(dog.legs[0].motorB, -ANGLE_B, KP3, KD3, 0);
            HT_SetPosition(dog.legs[1].motorB, ANGLE_B , KP3, KD3, 0);
            HT_SetPosition(dog.legs[2].motorB, ANGLE_B , KP3, KD3, 0);
            HT_SetPosition(dog.legs[3].motorB, -ANGLE_B, KP3, KD3, 0);*/

            jumpState = FALLING_STATE; // 落地状态中
        }
        break;

    default:
        break;
    }
}

void Jump2_Exit(void)
{
    UART_Print("Jump2 exit\n");
}